#include "action_nav_enemy.hpp"
#include "action_patrol.hpp"
#include  "condition_have_enemy.hpp"

#include <behaviortree_cpp_v3/bt_factory.h>
#include <behaviortree_cpp_v3/loggers/bt_cout_logger.h>
#include <behaviortree_cpp_v3/loggers/bt_zmq_publisher.h>

#include <iostream>    
#include <unistd.h>

#define DEFAULT_BT_XML "/home/ros/ws/src/turtle_bt_ros2/bt_xml/bt_npcturtle.xml"

using namespace BT;
static int flag4 = 0;

int main(int argc, char **argv) {
    usleep(1000000);
    setvbuf(stdout, NULL, _IONBF, BUFSIZ);
    rclcpp::init(argc, argv);
    rclcpp::Node::SharedPtr nh;

    if (flag4 == 0)  {
        // node_ = rclcpp::Node::make_shared("NavEnemy");
        nh = rclcpp::Node::make_shared("bt_turtle2_tree");
        flag4 = 1;
    }

    // auto nh = rclcpp::Node::make_shared("bt_turtle2_tree");
    nh->declare_parameter("bt_xml", rclcpp::ParameterValue(std::string(DEFAULT_BT_XML)));
    std::string bt_xml;
    nh->get_parameter("bt_xml", bt_xml);
    RCLCPP_INFO(nh->get_logger(), "Loading XML : %s", bt_xml.c_str());

    // We use the BehaviorTreeFactory to register our custom nodes
    BehaviorTreeFactory factory;

    factory.registerNodeType<HaveEnemy>("HaveEnemy");
    factory.registerNodeType<NavEnemy>("NavEnemy");
    factory.registerNodeType<NpcPatrol>("NpcPatrol");

    auto tree = factory.createTreeFromFile(bt_xml);

    // Create a logger
    StdCoutLogger logger_cout(tree);
    BT::PublisherZMQ publisher_zmq(tree);

    NodeStatus status = NodeStatus::RUNNING;
    // Keep on ticking until you get either a SUCCESS or FAILURE state
    while (rclcpp::ok() && status == NodeStatus::RUNNING) {
        // while (rclcpp::ok() ) {
        // while (rclcpp::ok() && ((status == NodeStatus::RUNNING)||(status == NodeStatus::SUCCESS))) {
        // while (1) {
            // rclcpp::spin(nh);
            status = tree.tickRoot();
            // Sleep 100 milliseconds
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        // }
    }

    return 0;
}